#舵机通道编号
#从底往上分别为：0~5
#摄像头控制：pitch - 6
import time
from servo import Servos
from machine import I2C, Pin

#设置舵机初始位置


arm_set = [90,90,90,90,90,90,90]

###########################################
#函数名：Arm_Set
#用途：设置舵机角度
#输入：ID：舵机通道编号，arm_reset：旋转角度
#输出：对应舵机当前角度值
###########################################
def Arm_Set(ID,arm_reset):
    while arm_set[ID]-arm_reset:
        if arm_set[ID] < arm_reset:
                arm_set[ID]+= 1
                servo.position(ID,arm_set[ID])


        elif  arm_set[ID] > arm_reset:
                arm_set[ID]-= 1
                servo.position(ID,arm_set[ID])



    return arm_set[ID]


i2c = I2C(sda=Pin('P8'), scl=Pin('P7'))
servo = Servos(i2c, address=0x40, freq=50, min_us=500, max_us=2500, degrees=180)

import time

status_stop =[[ 0,5],[1,179],[2,179],[3,179],[4,90],[5,0]]
status_long =[[ 0,0],[1,90],[2,95],[3,100],[4,0],[5,0]]
status_takeup_1 =[[ 0,5],[1,140],[2,80],[3,0],[4,0],[5,0]]
status_takeup_2 =[[ 0,5],[1,160],[2,80],[3,0],[4,0],[5,20]]
status_takeup_3=[[ 0,5],[1,90],[2,95],[3,100],[4,0],[5,20]]
status_putdowm_1 =[[ 0,100],[1,91],[2,70],[3,0],[4,0],[5,20]]
status_putdowm_2=[[ 0,100],[1,90],[2,95],[3,100],[4,0],[5,0]]

def run(motor_id,degree_now,degree_ref):
    if(degree_now<degree_ref):
        for t in range(degree_now ,degree_ref ):
            time.sleep(20)
            Arm_Set(motor_id,t)
    if(degree_now>degree_ref):
        for t in range(0,degree_now-degree_ref):
            time.sleep(20)
            Arm_Set(motor_id,degree_now-t)
def type_up():
    for i in range(0,6):
        run(i,status_stop[i][1],status_long[i][1])
def type_down():
    for i in range(0,6):
        run(i,status_long[i][1],status_stop[i][1])
def type_takeup():
    for i in range(0,6):
        run(i,status_long[i][1],status_takeup_1[i][1])
    time.sleep(20)
    run(1,140,160)
    time.sleep(20)
    servo.position(5,20)
    for i in range(0,6):
        run(i,status_takeup_2[i][1],status_takeup_3[i][1])
def type_putdowm():
    for i in range(0,6):
        run(i,status_takeup_3[i][1],status_putdowm_1[i][1])
    run(4,0,40)
    time.sleep(100)
    servo.position(5,0)
    time.sleep(50)
    for i in range(0,6):
        run(i,status_putdowm_1[i][1],status_putdowm_2[i][1])
    for i in range(0,6):
        run(i,status_putdowm_2[i][1],status_long[i][1])

def strong_stop():
    for i in range(0,6):
        Arm_Set(i,status_stop[i][1])

while 1:
    type_up()
    type_takeup()
    time.sleep(100)
    type_putdowm()
    type_down()
    strong_stop()


    break
while(1):
    a=1
    #Arm_Set(0,30)
    #Arm_Set(1,120)
    #Arm_Set(2,120)
    #Arm_Set(3,120)
    #Arm_Set(4,120)
    #Arm_Set(5,45)

